#include "stdafx.h"
#include "RigidBody.h"

#include "MyMath.h"

using namespace mphys;

#define LINEAR_DAMPING 0.95f
#define ANGULAR_DAMPING 0.75f

void RigidBody::addForce( XMVECTOR force )
{
	XMStoreFloat3(&ForceAccum, XMVectorAdd(XMLoadFloat3(&ForceAccum), force));
}

void RigidBody::addForceAtPoint( XMVECTOR force, XMVECTOR point ) // point in world coordinates (???)
{
	XMStoreFloat3(&ForceAccum, XMVectorAdd(XMLoadFloat3(&ForceAccum), force));
	
	XMVECTOR pointRelative = XMVectorSubtract(point, XMLoadFloat3(&Position));
	XMVECTOR torque = XMVector3Cross(pointRelative, force);
	XMStoreFloat3(&TorqueAccum, XMVectorAdd(XMLoadFloat3(&TorqueAccum), torque));
}

void RigidBody::clearAccums()
{
	ForceAccum = XMFLOAT3(0, 0, 0);
	TorqueAccum = XMFLOAT3(0, 0, 0);
}

void RigidBody::updateWorldMatrix()
{
	XMMATRIX matrix = XMMatrixRotationQuaternion(XMLoadFloat4(&Orientation));
	matrix = XMMatrixMultiply(matrix, XMMatrixTranslationFromVector(XMLoadFloat3(&Position)));
	XMStoreFloat4x4(&WorldMatrix, matrix);
}

void mphys::RigidBody::applyGravity(float dt)
{
	static const float GRAVITY_CONSTANT = 600;//3.0f;
	addForce(XMVectorSet(0.0f, -GRAVITY_CONSTANT * Mass, 0.0f, 0.0f));
}

void RigidBody::update( float dt )
{
	if(MassInverse == 0.0f)
		return;

	applyGravity(dt);
	updatePosition(dt);
	updateOrientation(dt);
	updateWorldMatrix();
	clearAccums();
}

// midpoint
void RigidBody::updatePosition( float dt )
{
	XMVECTOR velocity = XMLoadFloat3(&Velocity);
	XMVECTOR position = XMLoadFloat3(&Position);

	//XMVECTOR acc = XMLoadFloat3(&Acceleration);
	XMVECTOR acc = XMVectorMultiply(XMLoadFloat3(&ForceAccum), XMVectorReplicate(MassInverse));
	
	position = XMVectorMultiplyAdd(velocity, XMVectorReplicate(dt), position);
	position = XMVectorMultiplyAdd(acc, XMVectorReplicate(dt * dt * 0.5f), position); // pos = pos + vel * dt + acc * 0.5 * dt * dt - verlet


	XMVECTOR velocityChange = XMVectorMultiply(acc, XMVectorReplicate(dt));

	velocity = XMVectorAdd(velocity, velocityChange);

	velocity = XMVectorMultiply(velocity, XMVectorReplicate(powf(LINEAR_DAMPING, dt)));

	XMStoreFloat3(&Acceleration, acc);

	//position = XMVectorMultiplyAdd(
	//	XMVectorAdd(velocity, newVelocity),
	//	XMVectorReplicate(dt * 0.5f),
	//	position); // pos = (vel + newVel) * 0.5 * dt + pos - verlet integration

	XMStoreFloat3(&Velocity, velocity);
	XMStoreFloat3(&Position, position);

	XMStoreFloat3(&LastFrameVelocityChange, velocityChange);
	ContactResolutionPositionChange = XMFLOAT3(0.0f, 0.0f, 0.0f);
}
//
//void RigidBody::updatePosition( float dt )
//{
//	XMVECTOR velocity = XMLoadFloat3(&Velocity);
//	XMVECTOR position = XMLoadFloat3(&Position);
//
//	
//	XMVECTOR velocityChange = XMVectorMultiply(XMLoadFloat3(&ForceAccum), XMVectorReplicate(MassInverse * dt));
//
//	//velocity = XMVectorAdd(velocity, velocityChange); // vel = F/m * dt + vel
//	//velocity = XMVectorMultiply(velocity, XMVectorReplicate(powf(LINEAR_DAMPING, dt)));
//
//	//position = XMVectorMultiplyAdd(velocity, XMVectorReplicate(dt), position); // pos = vel * dt + pos
//
//	//XMStoreFloat3(&Velocity, velocity);
//	//XMStoreFloat3(&Position, position);
//
//	XMVECTOR newVelocity = XMVectorAdd(velocity, velocityChange); // vel = F/m * dt + vel
//	newVelocity = XMVectorMultiply(newVelocity, XMVectorReplicate(powf(LINEAR_DAMPING, dt)));
//
//	position = XMVectorMultiplyAdd(
//		XMVectorAdd(velocity, newVelocity),
//		XMVectorReplicate(dt * 0.5f),
//		position); // pos = (vel + newVel) * 0.5 * dt + pos - verlet integration
//
//	XMStoreFloat3(&Velocity, newVelocity);
//	XMStoreFloat3(&Position, position);
//
//	XMStoreFloat3(&LastFrameVelocityChange, velocityChange);
//	ContactResolutionPositionChange = XMFLOAT3(0.0f, 0.0f, 0.0f);
//}
//


void RigidBody::updateOrientation( float dt )
{
	XMVECTOR angularVelocity = XMLoadFloat3(&AngularVelocity);
	XMVECTOR orientation = XMLoadFloat4(&Orientation);

	// retrieve world rotation matrix and it's transpose
	XMMATRIX worldRotation = XMLoadFloat4x4(&WorldMatrix);
	worldRotation._41 = worldRotation._42 = worldRotation._43 = 0.0f;
	XMMATRIX worldRotationTranspose = XMMatrixTranspose(worldRotation);

	// transform inverse of inertia tensor to world space through IIT_W = R * IIT * R^T
	XMMATRIX worldInertiaTensorInverse = XMMatrixMultiply(worldRotation, XMLoadFloat3x3(&InertiaTensorInverse));
	worldInertiaTensorInverse = XMMatrixMultiply(worldInertiaTensorInverse, worldRotationTranspose);

	// get angular acceleration from torque accumulator using inverse of inertia tensor
	XMVECTOR angularAcc = XMVector3TransformNormal(XMLoadFloat3(&TorqueAccum), worldInertiaTensorInverse);

	// update velocity
	angularVelocity = XMVectorMultiplyAdd(angularAcc, XMVectorReplicate(dt), angularVelocity); // angVel = angAcc * dt + angVel
	angularVelocity = XMVectorMultiply(angularVelocity, XMVectorReplicate(powf(ANGULAR_DAMPING, dt)));

	// magic formula to integrate velocity into update of orientation: or = dt/2 * vector(angVel, 0) * or + or
	XMVECTOR orientationChange = XMVectorMultiply(angularVelocity, XMVectorReplicate(dt));
	orientationChange = XMVectorSetW(orientationChange, 0.0f);

	orientationChange = mmath::quaternionMultiply(orientationChange, orientation);

	orientation = XMVectorAdd(XMVectorMultiply(orientationChange, XMVectorReplicate(0.5f)), orientation);
	orientation = XMQuaternionNormalize(orientation); // fix inaccuracies

	XMStoreFloat3(&AngularVelocity, angularVelocity);
	XMStoreFloat4(&Orientation, orientation);
	XMStoreFloat3x3(&WorldSpaceInertiaTensorInverse, worldInertiaTensorInverse);
}

void RigidBody::initDynamic( const XMFLOAT3 & position, const XMFLOAT4 & orientation, float mass, const XMFLOAT3X3 & inertiaTensor )
{
	Position = position;
	Orientation = orientation;

	updateWorldMatrix();

	Mass = mass;
	MassInverse = 1.0f / mass;

	InertiaTensor = inertiaTensor;

	XMVECTOR tmp;
	XMStoreFloat3x3(&InertiaTensorInverse, XMMatrixInverse(&tmp, XMLoadFloat3x3(&inertiaTensor)));

	Velocity = XMFLOAT3(0, 0, 0);
	AngularVelocity = XMFLOAT3(0, 0, 0);
	Acceleration = XMFLOAT3(0, 0, 0);
	LastFrameVelocityChange = XMFLOAT3(0.0f, 0.0f, 0.0f);
	ContactResolutionPositionChange = XMFLOAT3(0.0f, 0.0f, 0.0f);
	clearAccums();
}

void mphys::RigidBody::initStatic( const XMFLOAT3 & position, const XMFLOAT4 & orientation )
{
	Position = position;
	Orientation = orientation;

	updateWorldMatrix();

	Mass = 0.0f;
	MassInverse = 0.0f; // special meaning - mass inverse of 0 means kinda 'inifnite mass', so it can't move

	ZeroMemory(&InertiaTensor, sizeof(InertiaTensor));
	ZeroMemory(&InertiaTensorInverse, sizeof(InertiaTensorInverse));
	
	Velocity = XMFLOAT3(0, 0, 0);
	AngularVelocity = XMFLOAT3(0, 0, 0);
	Acceleration = XMFLOAT3(0, 0, 0);
	LastFrameVelocityChange = XMFLOAT3(0.0f, 0.0f, 0.0f);
	ContactResolutionPositionChange = XMFLOAT3(0.0f, 0.0f, 0.0f);
	clearAccums();
}